package javax.robotics.engine;



/*
 * JRoboOp.java -- A Robotics Object Oriented Package in Java  
 * Copyright (c) 2005 Carmine Lia
 * e-mail:      carmine.lia@libero.it
 * web-address: http://www.intermedia.sa.it/lia

 *  This program is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU General Public License
 *  as published by the Free Software Foundation; either version 2
 *  of the License, or any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 *
 */

/**
 * <h2>Java Robotics Oriented Package.</h2>
 * <h3>A java wrapper for C++ library ROBOOP.</h3>
 * This class contains protected native methods.
 * <p>
 * To use it install the DLL library JRoboOp.dll (libJRoboOp.so for Linux)
 * </p>
 * 
 * @author <a href="http://www.intermedia.sa.it/lia/">Carmine Lia</a>
 * @version 1.1.2
 * 
 */
public abstract class JRoboOp
{
    // engine id
    protected static short ID = 0;
    public static final String Version  = "1.1.2";
    static
    {
        System.loadLibrary("JRoboOp");
    }
   
   
    /**
     * Retrieves engine ID from dll
     * @return the native engine ID
     */
    private native short setID();
    
    /**
     * Gets the ID.
     * @return ID
     */
    protected abstract short getID();
    /*
     * Gets converge.
     * 
     * @return converge flag
     */
   // protected abstract boolean getConverge();
    
    /*
     * Gets error.
     * 
     * @return error flag.
     *
    protected abstract short getError();
    */
    /*
     * ---------------------------------- Constructors
     */

    /**
     * Constructor of the engine.
     */
    protected JRoboOp()
    {
        ID = setID();
    }
       
    /*
     * ----------------------- class protected methods
     */
    /**
     * 
     * @param ID
     * @return error flag
     */
    protected native short getError(short ID);
    /**
     * @param ID
     * @return converge flag
     */
    protected native boolean getConverge(short ID);
    
//              Robot Contructors
    /**
     * Default Robot constructor.
     * 
     * <p>
     * Call C++ default Robot constructor
     * 
     * <pre>
     * <b>Robot</b>()
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     */
    protected native void createRobot(short ID);
    /**
     * Robot constructor with init parameter and motor parameter.
     * <p>
     * Call C++ Robot constructor
     * <pre>
     * <b>Robot</b>(const Matrix &amp;initRobot)
     * </pre>
     * </p>
     * @param ID engine id
     * @param initRobot
     *            vector of robot parameter; dimension: rowsR x colsR
     * @param rows
     *            rows of initRobot
     * @param cols
     *            columns of initRobot
     */
    protected native void createRobot(short ID, double[] initRobot, int rows, int cols);
    /**
     * Robot constructor with init parameter and motor parameter.
     * <p>
     * Call C++ Robot constructor
     * <pre>
     * <b>Robot</b>(const Matrix &amp;initRobot, const Matrix &amp;initMotor)
     * </pre>
     * 
     * </p>
     * @param ID engine id
     * @param initRobot
     *            vector of robot parameter; dimension: rowsR x colsR
     * @param rowsR
     *            rows of initRobot
     * @param colsR
     *            columns of initRobot
     * @param initMotor
     *            vector of motor robot parameter; dimension: rowsM x colsM
     * @param rowsM
     *            rows of initMotor
     * @param colsM
     *            columns of initMotor
     */
    protected native void createRobot(short ID, double[] initRobot, int rowsR, int colsR,
            double[] initMotor, int rowsM, int colsM);
    /**
     * Robot constructor with ROBOOP file parameter.
     * <p>
     * Call C++ Robot constructor
     * 
     * <pre>
     * <b>Robot</b>(const string &amp;fileName, const string &amp;robotName)</b>
     * </pre>
     * 
     * </p>
     * @param ID engine id
     * @param fileName
     *            configuration file name of robot parameter
     * @param robotName
     *            name of the robot
     */
    protected native void createRobot(short ID, String fileName, String robotName);
    
// Robot Kinematics   
    /**
     * Gets the degrees of freedom of robot.
     * <p>
     * Call C++ Robot method
     * <pre>
     * get_dof()
     * </pre>
     * </p>
     * @param ID engine id
     * @return degrees of freedom of robot manipulator
     */
    protected native int getDof(short ID);
    /**
     * Gets the available degrees of freedom of robot.
     * <p>
     * Call C++ Robot method
     * <pre>
     * get_available_dof()
     * </pre>
     * </p>
     * @param ID engine id
     * @return the available degrees of freedom of robot manipulator
     */
    protected native int getAvDof(short ID);
    
    /**
     * Gets joints maximum position.
     * <p>
     * Call C++ Robot method
     * <pre>
     * get_q_max()
     * </pre>
     * </p>
     * @param ID the engine id
     * @return the array of joints maximum position
     */
    protected native double[] getQMax(short ID);
    
    /**
     * Gets joints minumum position.
     * <p>
     * Call C++ Robot method
     * <pre>
     * get_q_min()
     * </pre>
     * </p>
     * @param ID the engine id
     * @return the array of joints minimum position
     */
    protected native double[] getQMin(short ID);
    
    /**
     * Gets joints minumum position.
     * <p>
     * Call C++ Robot method
     * <pre>
     * get_q_offset()
     * </pre>
     * </p>
     * @param ID the engine id
     * @return the array of joints offset position
     */
    protected native double[] getQOffset(short ID);
    
    /**
     * Gets joints position.
     * <p>
     * Call C++ Robot method
     * <pre>
     * get_q()
     * </pre>
     * </p>
     * @param ID the engine id
     * @return the array of joints position - joints offset
     */
    protected native double[] getQ(short ID);
    /**
     * Gets the position of i-th joint.
     * <p>
     * Call C++ Robot method
     * <pre>
     * get_q(int i)
     * </pre>
     * </p>
     * @param ID the engine id
     * @param i the joint.
     * @return the joint position - joint offset
     */
    protected native double getQ(short ID, int i);
    /**
     * Gets joints velocity.
     * <p>
     * Call C++ Robot method
     * <pre>
     * get_qp()
     * </pre>
     * </p>
     * @param ID the engine id
     * @return the array of joints velocity.
     */
    protected native double[] getQp(short ID);
    /*
     * Gets the velocity of i-th joint.
     * <p>
     * Call C++ Robot method
     * <pre>
     * get_qp(int i)
     * </pre>
     * </p>
     * @param ID the engine id
     * @param i the joint.
     * @return the joint velocity
     */
//    protected native double getQp(short ID, int i);
    /**
     * Gets joints acceleration.
     * <p>
     * Call C++ Robot method
     * <pre>
     * get_qpp()
     * </pre>
     * </p>
     * @param ID the engine id
     * @return the array of joints acceleration.
     */
    protected native double[] getQpp(short ID);
    /*
     * Gets the acceleration of i-th joint.
     * <p>
     * Call C++ Robot method
     * <pre>
     * get_qpp(int i)
     * </pre>
     * </p>
     * @param ID the engine id
     * @param i the joint.
     * @return the joint acceleration
     */
//    protected native double getQpp(short ID, int i);
    /**
     * Sets joints position adding the offset.
     * The new position is computed as joint position + joint offset.
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * set_q(const ColumnVector &amp;q)
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @param q
     *            the array of joints position
     */
    protected native void setQ(short ID, double[] q);
    /**
     * Sets i-th joint position adding the offset.
     * The new position is computed as joint position + joint offset. 
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * set_q(const Real q, int i)
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @param q
     *            joint position
     * @param i
     *            joint
     */
    protected native void setQ(short ID, double q, int i);
    /**
     * Sets joints velocity.
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * set_qp(const ColumnVector &amp;qp)
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @param qp
     *            array of joints velocity
     */
    protected native void setQp(short ID, double[] qp);
//    protected native void setQp(short ID, double qp, int i);
    /**
     * Sets joints acceleration.
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * set_qpp(const ColumnVector &amp;qpp)
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @param qpp
     *            the array of joints position
     */
    protected native void setQpp(short ID, double[] qpp);
//    protected native void setQpp(short ID, double qpp, int i);
    
    /**
     * Computes the solution of the Inverse Kinematics Problem from homogeneous
     * transform matrix using a Newton-Raphson technique.
     * 
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * inv_kin(const Matrix &amp;Tobj)
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @param Tobj
     *            elements of homogeneous transform matrix row major  
     * @return joints position
     */
    protected native double[] invKine(short ID, double[] Tobj);
    /**
     * Computes the solution of the Inverse Kinematics Problem from homogeneous
     * transform matrix using a Newton-Raphson or Taylor expansion techniques.
     * 
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * inv_kin(const Matrix &amp;Tob, const int mj, bool &amp;converge)
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @param Tobj
     *            elements homogeneous transform matrix row major.
     * @param mj
     *            value 0,1 to choose the technique: Newton-Raphson, Taylor
     *            expansion respectively.
     * @param endlink
     *            the link to pretend is the end effector.
     * @return joints position
     */
    protected native double[] invKine(short ID, double[] Tobj, int mj, int endlink);
    /**
     * Computes the Jacobian matrix expressed in the base frame.
     * 
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * jacobian()
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @return the array of jacobian matrix elements, the representation is row major.
     */
    protected native double[] jacobian(short ID);
    /**
     * Computes the Jacobian matrix expressed in the ref-th frame.
     * 
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * jacobian(const int ref)
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @param ref
     *            frame reference
     * @return the array of jacobian matrix elements, the representation is row major.
     */
    protected native double[] jacobian(short ID, int ref);
    /**
     * Computes the Jacobian time derivative matrix expressed in the base
     * frame.
     * 
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * jacobian_dot()
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @return the array of time derivative jacobian matrix elements, the representation is row major.
     */
    protected native double[] jacobianDot(short ID);

    /**
     * Computes the Jacobian time derivative matrix expressed in the ref-th
     * frame.
     * 
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * jacobian_dot(const int ref)
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @param ref
     *            frame reference
     * @return the array of time derivative jacobian matrix elements, the representation is row major.
     */
    protected native double[] jacobianDot(short ID, int ref);
    /**
     * Computes the 6x6 inverse Jacobian matrix based on the Damped
     * Least-Squares scheme.
     * 
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * jacobian_DLS_inv(const Real eps, const Real lambda_max)
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @param eps
     *            epsilon
     * @param lambdaMax
     *            damping factor
     * @return inverse jacobian matrix
     */
    protected native double[] jacobianDLSinv(short ID, double eps, double lambdaMax);
    /**
     * Computes the solution of the Forward Kinematics Problem.
     * 
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * kine()
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @return the elements of homogeneous transform matrix in row major representation.
     */
    protected native double[] kine(short ID);
    /**
     * Computes the solution of the Forward Kinematics Problem for the link j.
     * 
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * kine(const int j)
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @param j
     *            link of forward kinematic
     * @return the elements of homogeneous transform matrix in row major representation.
     */
    protected native double[] kine(short ID, int j);
    /**
     * Computes the partial derivative of homogeneous transform respect j-th
     * joint.
     * 
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * dTdqi(const int j)
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @param j
     *            joint
     * @return homogeneous partial derivative transform in row major representation.
     */
    protected native double[] dTdqi(short ID, int j);
    
// Robot Dynamics
    /**
     * Computes the solution of the Forward Dynamics Problem.
     * 
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * acceleration(const ColumnVector &amp;q, const ColumnVector &amp;qp, const ColumnVector &amp;tau)
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @param q
     *            joints position
     * @param qp
     *            joints velocity
     * @param tau
     *            joints torque
     * @return joints acceleration
     */
    protected native double[] acceleration(short ID, double[] q, double[] qp, double[] tau);
    /**
     * Computes the solution of the Forward Dynamics Problem with load applied
     * to the last link.
     * 
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * acceleration(const ColumnVector &amp;q, const ColumnVector &amp;qp, const ColumnVector &amp;tau_cmd, const ColumnVector &amp;Fext, const ColumnVector &amp;Next)
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @param q
     *            joints position
     * @param qp
     *            joints velocity
     * @param tauCmd
     *            joints torque
     * @param Fext
     *            load applied at last link
     * @param Next
     *            load applied at last link
     * @return joints acceleration
     */
    protected native double[] acceleration(short ID, double[] q, double[] qp,
            double[] tauCmd, double[] Fext, double[] Next);
    /**
     * Computes the solution of the Inverse Dynamics Problem.
     * 
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * torque(const ColumnVector &amp;q, const ColumnVector &amp;qp, const ColumnVector &amp;qpp)
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @param q
     *            joints position
     * @param qp
     *            joints velocity
     * @param qpp
     *            joints acceleration
     * @return joints torque
     */
    protected native double[] torque(short ID, double[] q, double[] qp, double[] qpp);
    /**
     * Computes the solution of the Inverse Dynamics Problem with load applied
     * to the last link.
     * 
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * torque(const ColumnVector &amp;q, const ColumnVector &amp;qp, const ColumnVector &amp;qpp, const ColumnVector &amp;Fext, const ColumnVector &amp;Next)
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @param q
     *            joints position
     * @param qp
     *            joints velocity
     * @param qpp
     *            joints acceleration
     * @param Fext
     *            load applied at last link
     * @param Next
     *            load applied at last link
     * @return joints torque
     */
    protected native double[] torque(short ID, double[] q, double[] qp, double[] qpp,
            double[] Fext, double[] Next);
    /**
     * Computes the torques from the gravity effect.
     * 
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * G()
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @return joints torque
     */
    protected native double[] torqueGravity(short ID);
    /**
     * Computes the torques from the Coriolis and centrifugal effect.
     * 
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * C(CoulumnVector & qp)
     * </pre>
     * Note that the original roboop C() function differs from this one, it includes also the torques due to viscous and coulomb friction.  
     * </p>
     * @param ID the engine id
     * @param qp
     *            joints velocity
     * @return joints torque
     */
    protected native double[] torqueCoriolis(short ID, double[] qp);
    
    /**
     * Computes the torques from the viscous friction.
     * 
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * torqueViscousFriction(CoulumnVector & qp)
     * </pre>
     * </p>
     * @param ID the engine id
     * @param qp
     *            joints velocity
     * @return joints torque
     */
    protected native double[] torqueViscousFriction(short ID, double[] qp);
    
    /**
     * Computes the torques from the Coulomb friction.
     * 
     * <p>
     * Call C++ Robot method
     * <pre>
     * torqueCoulombFriction(CoulumnVector & qp)
     * </pre>
     * </p>
     * @param ID the engine id
     * @param qp
     *            joints velocity
     * @return joints torque
     */
    protected native double[] torqueCoulombFriction(short ID, double[] qp);
    
    /**
     * Computes the inertia matrix.
     * 
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * inertia(const ColumnVector &amp;q)
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @param q
     *            joints position
     * @return intertia matrix
     */
    protected native double[] inertia(short ID, double[] q);
    /**
     * Gets gravity vector.
     * <p>
     * return the <code>robot.gravity</code> parameter.
     * </p>
     * @param ID the engine id
     * @return the gravity vector
     */
    protected native double[] getGravity(short ID);
    /**
     * Sets the gravity vector.
     * <p>
     * sets the <code>robot.gravity</code> parameter.
     * </p>
     * @param ID the engine id
     * @param gravity the new gravity vector.
     */
    protected native void setGravity(short ID, double[] gravity);

// Robot CLIK
//    protected native void createCLIK(short ID, double[] Kp, double[] Ko, double dt);
//    protected native void createCLIK(short ID, double[] Kp, double[] Ko, double eps, double lambdaMax, double dt);
//    protected native void createCLIK(short ID, double[] Kp, double dt, int algorithm);
//    protected native double[] qQdot(short ID, double[] pd, double[] pddot);
//    protected native double[] qQdot(short ID, double[] quatd, double[] pd, double[] pddot, double[] wd);
    
// Controllers
    /*                        ------------------------
     *                         Proportional derivative 
     *                      -----------------------------
     */  
    /**
     * Proportional Derivative  joints position controller class constructor.
     * <p>
     * Call C++ constructor
     * <pre>
     * Proportional_derivative(const Robot_basic &amp;robot, const DiagonalMatrix &amp;Kp, const DiagonalMatrix &amp;Kd)
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @param robotID the robot id
     * @param Kp
     *            diagonal matrix joint position error gain matrix.
     * @param Kd
     *            diagonal matrix joint velocity error gain matrix.
     */
    protected native void createProportionalDerivative(short ID, short robotID, double[] Kp, double[] Kd);
    /**
     * Sets the output torque for a desired joint position and velocity.
     * <p>
     * Call C++ Proportional_derivative method
     * <pre>
     * torque_cmd(Robot_basic &amp;robot, const ColumnVector &amp;qd)
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @param robotID the robot id
     * @param qd array of desired joint position and desired joint velocity.
     * @return torque.
     */
    protected native double[] torqueCmdPD(short ID, short robotID, double[] qd, double[] qpd);
    /**
     * Sets the position error gain matrix.
     * <p>
     * Call C++ Proportional_derivative method <pre>set_Kp(const DiagonalMatrixr &Kp)</pre>
     * </p>
     * @param ID the engine id
     * @param robotID the robot id
     * @param Kp position error gain diagonal matrix
     * @return 0 or WRONG_SIZE
     */
    protected native short setKpPD(short ID, short robotID, double[] Kp);
    /**
     * Sets the velocity error gain matrix.
     * <p>
     * Call C++ Proportional_derivative method <pre>set_Kd(const DiagonalMatrixr &Kd)</pre>
     * </p>
     * @param ID the engine id
     * @param robotID the robot id
     * @param Kd velocity error gain diagonal matrix
     * @return 0 or WRONG_SIZE
     */
    protected native short setKdPD(short ID, short robotID, double[] Kd);
    
    /*                        ------------------------
     *                Proportional derivative with gravity compensation
     *                      -----------------------------
     */   
    /**
     * Proportional Derivative with gravity compensation joints position controller class constructor.
     * <p>
     * Call C++ constructor
     * <pre>
     * PDGravityComp(const Robot_basic &amp;robot, const DiagonalMatrix &amp;Kp, const DiagonalMatrix &amp;Kd)
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @param robotID the robot id
     * @param Kp
     *            diagonal matrix joint position error gain matrix.
     * @param Kd
     *            diagonal matrix joint velocity error gain matrix.
     */
    protected native void createPDGravityComp(short ID, short robotID, double[] Kp, double[] Kd);
    /**
     * Sets the output torque for a desired joint position and velocity.
     * <p>
     * Call C++ PDGravityComp method
     * <pre>
     * torque_cmd(Robot_basic &amp;robot, const ColumnVector &amp;qd)
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @param robotID the robot id
     * @param qd array of desired joint position and desired joint velocity.
     * @return torque.
     */
    protected native double[] torqueCmdPDG(short ID, short robotID, double[] qd);
    /**
     * Sets the position error gain matrix.
     * <p>
     * Call C++ PDGravityComp method <pre>set_Kp(const DiagonalMatrixr &Kp)</pre>
     * </p>
     * @param ID the engine id
     * @param robotID the robot id
     * @param Kp position error gain diagonal matrix
     * @return 0 or WRONG_SIZE
     */
    protected native short setKpPDG(short ID, short robotID, double[] Kp);
    /**
     * Sets the velocity error gain matrix.
     * <p>
     * Call C++ PDGravityComp method <pre>set_Kd(const DiagonalMatrixr &Kd)</pre>
     * </p>
     * @param ID the engine id
     * @param robotID the robot id
     * @param Kd velocity error gain diagonal matrix
     * @return 0 or WRONG_SIZE
     */
    protected native short setKdPDG(short ID, short robotID, double[] Kd);
    
/*                        ------------------------
 *                         Computed Torque Method
 *                      -----------------------------
 */   
    /**
     * Computed Torque Method position controller class constructor.
     * <p>
     * Call C++ constructor
     * 
     * <pre>
     * Computed_torque_method(const Robot_basic &amp;robot, const DiagonalMatrix &amp;Kp, const DiagonalMatrix &amp;Kd)
     * </pre>
     * 
     * </p>
     * @param ID the engine id
     * @param robotID the robot id
     * @param Kp
     *            diagonal matrix joint position error gain matrix.
     * @param Kd
     *            diagonal matrix joint velocity error gain matrix.
     */
    protected native void createComputedTorqueMethod(short ID, short robotID, double[] Kp, double[] Kd);
    /**
     * Sets the output torque for a desired joint position and velocity.
     * <p>
     * Call C++ Computed_torque_method method
     * <pre>
     *  torque_cmd(Robot_basic &amp;robot, const ColumnVector &amp;qd, const ColumnVector &amp;qpd, const ColumnVector &amp;qppd)
     * </pre>
     * </p>
     * 
     * @param qd desired joints position
     * @param qpd desired joints velocity
     * @param qppd desired joints acceleration
     * @return the output torque.
     */
    protected native double[] torqueCmdCTM(short ID, short robotID, double[] qd, double[] qpd, double[] qppd);
    /**
     * Sets the position error gain matrix.
     * <p>
     * Call C++ Computed_torque_method method <pre>set_Kp(const DiagonalMatrixr &Kp)</pre>
     * </p>
     * @param ID the engine id
     * @param robotID the robot id
     * @param Kp position error gain diagonal matrix
     * @return 0 or WRONG_SIZE
     */
    protected native short setKpCTM(short ID, short robotID, double[] Kp);
    /**
     * Sets the position error gain matrix.
     * <p>
     * Call C++ Computed_torque_method method <pre>set_Kd(const DiagonalMatrixr &Kd)</pre>
     * </p>
     * @param ID the engine id
     * @param robotID the robot id
     * @param Kd position error gain diagonal matrix
     * @return 0 or WRONG_SIZE
     */
    protected native short setKdCTM(short ID, short robotID, double[] Kd);
 
    /*                        ------------------------
     *                            Robust control
     *                      -----------------------------
     */   
        /**
         * Robust control position controller class constructor.
         * <p>
         * Call C++ constructor
         * 
         * <pre>
         *  Robust_control(const Robot_basic &amp;robot, const DiagonalMatrix &amp;Kp, const DiagonalMatrix &amp;Kd, const Matrix &amp;B, const Matrix &amp;Q, double rho, double eps)
         * </pre>
         * 
         * </p>
         * @param ID the engine id
         * @param robotID the robot id
         * @param Kp
         *            diagonal matrix joint position error gain matrix.
         * @param Kd
         *            diagonal matrix joint velocity error gain matrix.
         * @param B
         *            constant inertia matrix of robot. 
         * @param Q
         *            the solution of Lyapunov equation. 
         * @param rho
         *            the robustness gain.
         * @param eps
         *            the thickness of robustness region to compensate the chattering.
         */
        protected native void createRobustControl(short ID, short robotID, double[] Kp, double[] Kd, double[] B, double[] Q, double rho, double eps);
        
        /**
         * Sets the output torque for a desired joint position and velocity.
         * <p>
         * Call C++ Robust_control method
         * <pre>
         *  torque_cmd(Robot_basic &amp;robot, const ColumnVector &amp;qd, const ColumnVector &amp;qpd, const ColumnVector &amp;qppd)
         * </pre>
         * </p>
         * 
         * @param qd desired joints position
         * @param qpd desired joints velocity
         * @param qppd desired joints acceleration
         * @return the output torque.
         */
        protected native double[] torqueCmdRC(short ID, short robotID, double[] qd, double[] qpd, double[] qppd);
        /**
         * Sets the position error gain matrix.
         * <p>
         * Call C++ Robust_control method <pre>set_Kp(const DiagonalMatrixr &Kp)</pre>
         * </p>
         * @param ID the engine id
         * @param robotID the robot id
         * @param Kp position error gain diagonal matrix
         * @return 0 or WRONG_SIZE
         */
        protected native short setKpRC(short ID, short robotID, double[] Kp);
        /**
         * Sets the position error gain matrix.
         * <p>
         * Call C++ Robust_control method <pre>set_Kd(const DiagonalMatrixr &Kd)</pre>
         * </p>
         * @param ID the engine id
         * @param robotID the robot id
         * @param Kd position error gain diagonal matrix
         * @return 0 or WRONG_SIZE
         */
        protected native short setKdRC(short ID, short robotID, double[] Kd);

    /*                        ------------------------
     *                       Resolved Rate Acceleration
     *                      -----------------------------
     */   
    /**
     * Resolved Rate Acceleration controller class constructor.
     * <p>
     * Call C++ constructor <pre>Resolved_acc(const Robot_basic &robot, 
     *                               double Kvp, double  Kpp,
     *                               double Kvo, double Kpo)</pre>
     * </p>
     * @param ID the engine id
     * @param robotID the robot id
     * @param Kvp end effector velocity error gain
     * @param Kpp end effector position error gain
     * @param Kvo end effector orientation angular rate gain
     * @param Kpo end effector orientation error gain
     */
    protected native void createResolveRateAcceleration(short ID, short robotID, double Kvp, double Kpp, double Kvo, double Kpo);
    /**
     * Gets the position error.
     * <p>
     * Call C++ Resolved_acc method <pre>get_pos_error()</pre>
     * </p>
     * @param ID the engine id
     * @return the pos error array.
     */
    protected native double[] getPosErrorRRA(short ID);
    
    /**
     * Gets the quaternion error.
     * <p>
     * Call C++ Resolved_acc method <pre>get_quat_error()</pre>
     * </p>
     * @param ID the engine id
     * @return the quaternion error array.
     */
    protected native double[] getQuatErrorRRA(short ID);
    
    /**
     * 
     * Sets the output torque for a desired end effector: acceleration, velocity, position, angular acceleration, angular velocity, angular position.
     * <p>
     * Call C++ Resolved_acc method <pre>torque_cmd(Robot_basic &robot, 
     *                                     const ColumnVector &pdpp const ColumnVector &pdp,
     *                                     const ColumnVector &pd, const ColumnVector &wdp,
     *                                     const ColumnVector &wd, const Quaternion &qd,
     *                                     const short link_pc, const Real dt)</pre>
     * </p>
     * @param ID the engine id
     * @param robotID the robot id
     * @param pdpp desired acceleration
     * @param pdp desired velocity
     * @param pd desired position
     * @param wdp desired angular acceleration
     * @param wd desired angular velocity
     * @param qd desired vector angular position: quaternion (s,v1,v2,v3)
     * @param linkPc link of which control position
     * @return the output torque
     */
    protected native double[] torqueCmdRRA(short ID, short robotID, double[] pdpp, double[] pdp, double[] pd, double[] wdp, double[] wd, double[] qd, short linkPc);
    /**
     * 
     * Sets the output torque for a desired end effector: acceleration, velocity, position.
     * <p>
     * Call C++ Resolved_acc method <pre>torque_cmd(Robot_basic &robot, 
     *                                     const ColumnVector &pdpp const ColumnVector &pdp,
     *                                     const ColumnVector &pd)</pre>
     * </p>
     * @param ID the engine id
     * @param robotID the robot id
     * @param pdpp desired acceleration
     * @param pdp desired velocity
     * @param pd desired position
     * @return the output torque
     */
    protected native double[] torqueCmdRRA(short ID, short robotID, double[] pdpp, double[] pdp, double[] pd);
    
    /*                        ------------------------
     *                          Gravity Compensation
     *                      -----------------------------
     */   
    /**
     * Gravity Compensation controller class constructor.
     * <p>
     * Call C++ constructor <pre>Gravity_comp(const Robot_basic &robot, 
     *                               double Kvp, double  Kpp,
     *                               double Kvo, double Kpo)</pre>
     * </p>
     * @param ID the engine id
     * @param robotID the robot id
     * @param Kvp end effector velocity gain
     * @param Kpp end effector position error gain
     * @param Kvo end effector orientation angular rate gain
     * @param Kpo end effector orientation error gain
     */
    protected native void createGravityCompensation(short ID, short robotID, double Kvp, double Kpp, double Kvo, double Kpo);
   
    /**
     * Gets the position error.
     * <p>
     * Call C++ Gravity_comp method <pre>get_pos_error()</pre>
     * </p>
     * @param ID the engine id
     * @return the pos error array.
     */
    protected native double[] getPosErrorGCOMP(short ID);
    
    /**
     * Gets the quaternion error.
     * <p>
     * Call C++ Gravity_comp method <pre>get_quat_error()</pre>
     * </p>
     * @param ID the engine id
     * @return the quaternion error array.
     */
    protected native double[] getQuatErrorGCOMP(short ID);
    
    /**
     * 
     * Sets the output torque for a desired end effector position and angular position.
     * <p>
     * Call C++ Gravity_comp method <pre>torque_cmd(Robot_basic &robot,
     *                                     const ColumnVector &pd, const Quaternion &qd,
     *                                     const short link_pc, const Real dt)</pre>
     * </p>
     * @param ID the engine id
     * @param robotID the robot id
     * @param pd desired position
     * @param qd desired vector angular position: quaternion (s,v1,v2,v3)
     * @param linkPc link of which control position
     * @return the output torque
     */
    protected native double[] torqueCmdGCOMP(short ID, short robotID, double[] pd, double[] qd, short linkPc);
    /**
     * 
     * Sets the output torque for a desired end effector position.
     * <p>
     * Call C++ Gravity_comp method <pre>torque_cmd(Robot_basic &robot,
     *                                     const ColumnVector &pd)</pre>
     * </p>
     * @param ID the engine id
     * @param robotID the robot id
     * @param pd desired position
     * @return the output torque
     */
    protected native double[] torqueCmdGCOMP(short ID, short robotID, double[] pd);
    
    /**
     * @param ID
     * @param robotID
     * @param Mp
     * @param Dp
     * @param Kp
     * @param Km
     * @param Mo
     * @param Do
     * @param Ko
     */
    protected native void createImpedance(short ID, short robotID, double[] Mp, double[] Dp, double[] Kp, double[] Km, double[] Mo, double[] Do, double[] Ko);
    /**
     * @param ID
     * @param robotID
     * @param pdpp
     * @param pdp
     * @param pd
     * @param wdp
     * @param wd
     * @param qd
     * @param f
     * @param n
     * @param dt
     * @return null
     */
    protected native short impedanceControl(short ID, short robotID, double[] pdpp, double[] pdp, double[] pd, double[] wdp, double[] wd, double[] qd, double[] f, double[] n, double dt);

// Trajectory
    /*
     * Spline Path class constructor from ROBOOP file.
     * <p>
     * Parametric cubic splines interpolation of points in cartesian space or
     * joints space, needs at least four points. <br>
     * Example of files:<br>
     * 
     * <pre>
     * 
     *  
     *  # &lt;comments&gt;
     *  CARTESIAN_SPACE
     *  3                  cartesian space dimension
     *  0                  time
     *  0.0 0.5 0.0        x,y,z
     *  0.25          
     *  0.0732 0.6768 0.0327
     *  0.5          
     *  0.25 0.75 0.0654
     *  ... 
     *              
     *  # &lt;comments&gt;
     *  JOINTS_SPACE
     *  4                  joints space dimension
     *  0                  time
     *  0.0 0.0 0.0 0.0    q1, q2, q3, q4
     *  0.25          
     *  0.1 0.1 0.1 0.1
     *  ...  
     *   
     *  
     * </pre>
     * 
     * </p>
     * Call C++ constructor
     * 
     * {@code Spl_path(const string &amp;filename)}
     * 
     * @param ID the engine id.
     * @param filename
     *            file of points
     * @return the final time.
     *
    protected native double createSplinePath(short ID, String filename);
    */
    /**
     * Spline Path class constructor from ROBOOP file.
     * <p>
     * Parametric cubic splines interpolation of points in cartesian space or
     * joints space, needs at least four points. <br>
     * </p>
     * Call C++ constructor
     * 
     * {@code Spl_path(const Matrix &amp;x)}
     * 
     * @param ID the engine id.
     * @param spaceDim the dimension of cartesian space or th joints space.
     * @param nPoints the number of points of trajectory
     * @param array the (spaceDim+1)-by-npoints array: 1st row the times, 2nd and next the points 
     */
    protected native void createSplinePath(short ID, int spaceDim, int nPoints, double[] array);
    /**
     * Interpolates the spline at time t to set the position, velocity and
     * acceleration.
     * 
     * <p>
     * Call C++ Spl_path method
     * <pre>p_pdot_pddot(const Real time, ColumnVector &amp;p, ColumnVector &amp;dp, ColumnVector &amp;ddp)</pre>
     * </p>
     * <p>
     * It modifies error flag.
     * </p>
     * @param ID the engine id.
     * @param t
     *            time
     * @return  the array p & pp & ppp
     */
    protected native double[] interpolate(short ID, double t);
    /*
     * Spline Quaternion class constructor from ROBOOP file.
     * 
     * <p>
     * Parametric cubic splines interpolation of quaternions. <br>
     * Example of file:<br>
     * 
     * <pre>
     * 
     *              # &lt;comments&gt;
     *              0                                      time
     *              R                                      rotation matrix
     *              1.0 0.0 0.0 0.0 -1.0 0.0 0.0 0.0 -1.0  r11, r12, r13, r21, ... 
     *              0.25                                   time
     *              Q                                      quaternion          
     *              0.0732 0.6768 0.0327 0.0323            s, v1 ,v2, v3 
     *              ... 
     *  
     * </pre>
     * 
     * </p>
     * <p>
     * Call C++ constructor
     * 
     * <br>
     * <code>Spl_Quaternion(const string &amp;filename)</code>
     * </p>
     * @param ID the engine id. 
     * @param filename
     *            file of quaternions or rotations
     * @return the final time.
     *
    protected native double createSplineQuaternion(short ID, String filename);
    */
    /**
     * Spline Quaternion class constructor from array.
     * 
     * <p>
     * Parametric cubic splines interpolation of quaternions. <br>
     * </p>
     * <p>
     * Call C++ constructor
     * <br>
     * <code>Spl_Quaternion(const quat_map &amp;quat)</code>
     * </p>
     * @param ID the engine id.
     * @param nPoints the number of trajectory points. 
     * @param array the 5-by-nPoints array: [1st point (time s vx vy vz)] & [2nd point (time s vx vy vz)] & ...
     */
    protected native void createSplineQuaternion(short ID, int nPoints, double[] array);
    /**
     * Interpolates the spline at time t to set the quaternion and angular
     * velocity.
     * <p>
     * Call C++ Spl_Quaternion method
     * 
     * <pre>
     *  quat_w(const Real time, Quaternion &amp; q, ColumnVector &amp;wd)
     * </pre>
     * 
     * </p>
     * <p>
     * It modifies error flag.
     * </p>
     * @param ID the engine id.
     * @param t
     *            time
     * @return the array q & wd 
     */
    protected native double[] interpolateQuat(short ID, double t);
    /** 
     * Interpolates the spline at time t to set the position, velocity,
     * acceleration, the quaternion and angular velocity.
     * <p>
     * Call C++ Spl_path and Spl_Quaternion methods:
     * <pre>
     * p_pdot_pddot(const Real time, ColumnVector &amp;p, ColumnVector &amp;dp, ColumnVector &amp;ddp)
     * quat_w(const Real time, Quaternion &amp; q, ColumnVector &amp;wd)
     * </pre>
     *</p>
     * <p>
     * It modifies error flag.
     * </p>
     * @param ID the engine id.
     * @param t the time
     * @return the array of p & pp & ppp & q & wd
     */
    protected native double[] interpolatePathQuat(short ID, double t);
}

